#include "IsolationCalculator.h"
#include "MoMFieldSolver.h"
#include <cmath>
#include <stdexcept>

namespace FieldCircuitCoupling {

IsolationCalculator::IsolationCalculator(MoMFieldSolver* solver) : solver(solver) {
}

IsolationCalculator::~IsolationCalculator() {
}

// Compute isolation matrix
bool IsolationCalculator::computeIsolationMatrix(const std::vector<CouplingPort>& ports, 
                                              std::vector<std::vector<Complex>>& isolationMatrix) {
    /**
     * Algorithm principle:
     * 1. For each port i as the excitation source, calculate the response of all other ports j
     * 2. Port isolation is defined as: S_ij = V_j / V_i (in normalized case)
     * 3. Consider phase matching under periodic boundary conditions
     */
    size_t numPorts = ports.size();
    isolationMatrix.resize(numPorts, std::vector<Complex>(numPorts, 0.0));
    
    // Cache original solution vector
    std::vector<Complex> originalSolution = solver->solutionVector;
    
    try {
        // For each port as the excitation source
        for (size_t i = 0; i < numPorts; ++i) {
            // Create excitation vector for port i
            std::vector<Complex> portExcitation(solver->numUnknowns, 0.0);
            
            // Calculate impedance values for port i
            std::vector<Complex> portImpedance;
            if (!computePortImpedance(ports[i], portImpedance)) {
                solver->solutionVector = originalSolution;
                return false;
            }
            
            // Apply normalized port excitation
            for (size_t j = 0; j < portImpedance.size() && j < portExcitation.size(); ++j) {
                if (std::abs(portImpedance[j]) > 1e-12) {
                    portExcitation[j] = std::conj(portImpedance[j]) / 
                                      (std::norm(portImpedance[j]) + ports[i].impedance * 
                                      std::conj(portImpedance[j]));
                }
            }
            
            // Solve the field problem under current excitation
            if (!solver->solve(portExcitation)) {
                solver->solutionVector = originalSolution;
                return false;
            }
            
            // Calculate responses for all ports
            for (size_t j = 0; j < numPorts; ++j) {
                // Calculate impedance values for port j
                std::vector<Complex> observePortImpedance;
                if (!computePortImpedance(ports[j], observePortImpedance)) {
                    solver->solutionVector = originalSolution;
                    return false;
                }
                
                // Calculate induced current on port j (dot product of solution vector and port impedance vector)
                Complex isolation = 0.0;
                for (size_t k = 0; k < solver->solutionVector.size() && k < observePortImpedance.size(); ++k) {
                    isolation += solver->solutionVector[k] * std::conj(observePortImpedance[k]);
                }
                
                // Consider the effect of periodic boundary
                if (solver->periodicManager->isPeriodicBoundaryEnabled()) {
                    PeriodicBoundary::PeriodicConfig config;
                    if (solver->periodicManager->getPeriodicConfig(config)) {
                        Complex periodicFactor = 1.0;
                        Vector_3D relativePosition = ports[j].position - ports[i].position;
                        
                        switch (config.type) {
                            case PeriodicBoundary::PeriodicType::PHASE_MATCHED:
                                periodicFactor = std::exp(Complex(0, 1) * 
                                    (config.kx * relativePosition.x + config.ky * relativePosition.y + config.kz * relativePosition.z));
                                break;
                            case PeriodicBoundary::PeriodicType::FLOQUET: {
                                Vector_3D normalizedPosition;
                                normalizedPosition.x = std::fmod(relativePosition.x, config.x2 - config.x1);
                                normalizedPosition.y = std::fmod(relativePosition.y, config.y2 - config.y1);
                                normalizedPosition.z = std::fmod(relativePosition.z, config.z2 - config.z1);

                                periodicFactor = std::exp(Complex(0, 1) *
                                    (config.phaseShiftX * normalizedPosition.x + config.phaseShiftY * normalizedPosition.y +
                                        config.phaseShiftZ * normalizedPosition.z)); }
                                break;
                            default:
                                break;
                        }
                        
                        isolation *= periodicFactor;
                    }
                }
                
                // For self-isolation (i=j), consider the effect of impedance matching
                if (i == j) {
                    // Compute port input impedance
                    Complex zIn = 0.0;
                    for (const auto& z : portImpedance) {
                        zIn += z;
                    }
                    
                    // Calculate normalization factor based on port characteristic impedance
                    Complex z0 = ports[i].impedance;
                    if (std::abs(zIn + z0) > 1e-12) {
                        isolation = (4.0 * std::norm(z0)) / std::norm(zIn + z0);
                    }
                }
                
                isolationMatrix[i][j] = isolation;
            }
        }
        
        // Restore original solution vector
        solver->solutionVector = originalSolution;
        return true;
    } catch (...) {
        // Restore original solution vector
        solver->solutionVector = originalSolution;
        return false;
    }
}

// Calculate S-parameter matrix
bool IsolationCalculator::computeSParameters(const std::vector<CouplingPort>& ports, 
                                           std::vector<std::vector<Complex>>& sMatrix) {
    /**
     * Algorithm principle:
     * Relationship between S-parameters and isolation matrix: S_ij = sqrt(Pj/Pi) * isolation_ij
     * where Pi is the input power at port i, and Pj is the received power at port j
     */
    size_t numPorts = ports.size();
    sMatrix.resize(numPorts, std::vector<Complex>(numPorts, 0.0));
    
    // First calculate isolation matrix
    std::vector<std::vector<Complex>> isolationMatrix;
    if (!computeIsolationMatrix(ports, isolationMatrix)) {
        return false;
    }
    
    // Calculate normalization factors and construct S-parameter matrix
    for (size_t i = 0; i < numPorts; ++i) {
        // Calculate input power at port i (based on characteristic impedance)
        double powerInput = std::norm(ports[i].impedance);
        
        for (size_t j = 0; j < numPorts; ++j) {
            // Calculate received power at port j
            double powerOutput = std::norm(ports[j].impedance);
            
            // Calculate normalization factor
            double normFactor = std::sqrt(powerOutput / powerInput);
            
            // Construct S-parameter matrix
            sMatrix[i][j] = isolationMatrix[i][j] * normFactor;
        }
    }
    
    return true;
}

// Convert isolation to dB value
double IsolationCalculator::computeIsolationInDB(const Complex& isolation) {
    double magnitude = std::abs(isolation);
    if (magnitude < 1e-12) {
        return -100.0; // Minimum isolation limit
    }
    return 10.0 * std::log10(magnitude);
}

// Calculate isolation between two specific ports
bool IsolationCalculator::computePortIsolation(const CouplingPort& sourcePort, const CouplingPort& observePort, Complex& isolation) {
    /**
     * Calculate isolation from source port to observation port
     * This method is more efficient than calculating the entire isolation matrix and is suitable for cases where only specific port pairs are needed
     */
    if (!solver->isInitialized) {
        return false;
    }
    
    // Cache current solution vector
        std::vector<Complex> originalSolution = solver->solutionVector;
        
        try {
            // Create excitation vector for source port
            std::vector<Complex> portExcitation(solver->numUnknowns, 0.0);
            
            // Calculate impedance values for source port to generate normalized excitation
            std::vector<Complex> sourcePortImpedance;
            if (!computePortImpedance(sourcePort, sourcePortImpedance)) {
                solver->solutionVector = originalSolution;
                return false;
            }
        
        // Apply normalized port excitation
        for (size_t i = 0; i < sourcePortImpedance.size() && i < portExcitation.size(); ++i) {
            if (std::abs(sourcePortImpedance[i]) > 1e-12) {
                portExcitation[i] = std::conj(sourcePortImpedance[i]) / 
                                  (std::norm(sourcePortImpedance[i]) + sourcePort.impedance * 
                                  std::conj(sourcePortImpedance[i]));
            }
        }
        
        // Solve the field problem under current excitation
        if (!solver->solve(portExcitation)) {
            solver->solutionVector = originalSolution;
            return false;
        }
        
        // Calculate impedance values for observation port
        std::vector<Complex> observePortImpedance;
        if (!computePortImpedance(observePort, observePortImpedance)) {
            solver->solutionVector = originalSolution;
            return false;
        }
        
        // Calculate induced current on observation port
        isolation = 0.0;
        for (size_t i = 0; i < solver->solutionVector.size() && i < observePortImpedance.size(); ++i) {
            isolation += solver->solutionVector[i] * std::conj(observePortImpedance[i]);
        }
        
        // Consider the effect of periodic boundaries
        if (solver->periodicManager->isPeriodicBoundaryEnabled()) {
            PeriodicBoundary::PeriodicConfig config;
            if (solver->periodicManager->getPeriodicConfig(config)) {
                Complex periodicFactor = 1.0;
                Vector_3D portPosition = observePort.position;
                
                // Calculate port position offset relative to source port
                Vector_3D relativePosition = portPosition - sourcePort.position;
                
                switch (config.type) {
                    case PeriodicBoundary::PeriodicType::PHASE_MATCHED:
                        // For phase-matched boundaries, calculate phase shift using relative position
                        periodicFactor = std::exp(Complex(0, 1) * 
                            (config.kx * relativePosition.x + config.ky * relativePosition.y + config.kz * relativePosition.z));
                        break;
                    case PeriodicBoundary::PeriodicType::FLOQUET: {
                        // For Floquet boundaries, consider grid period and phase shift
                        Vector_3D normalizedPosition;
                        normalizedPosition.x = std::fmod(relativePosition.x, config.x2 - config.x1);
                        normalizedPosition.y = std::fmod(relativePosition.y, config.y2 - config.y1);
                        normalizedPosition.z = std::fmod(relativePosition.z, config.z2 - config.z1);

                        periodicFactor = std::exp(Complex(0, 1) *
                            (config.phaseShiftX * normalizedPosition.x + config.phaseShiftY * normalizedPosition.y +
                                config.phaseShiftZ * normalizedPosition.z)); }
                        break;
                    case PeriodicBoundary::PeriodicType::UNIT_CELL:
                        // For identical phase boundaries, ensure phase consistency when field repeats periodically
                        // No additional phase factor is typically needed in this case, but kept as an extension point
                        break;
                    default:
                        break;
                }
                
                isolation *= periodicFactor;
            }
        }
        
        // If self-isolation, consider impedance matching
        if (sourcePort.portId == observePort.portId) {
            // Compute port input impedance
            Complex zIn = 0.0;
            for (const auto& z : sourcePortImpedance) {
                zIn += z;
            }
            
            // Calculate normalization factor based on port characteristic impedance
            Complex z0 = sourcePort.impedance;
            if (std::abs(zIn + z0) > 1e-12) {
                isolation = (4.0 * std::norm(z0)) / std::norm(zIn + z0);
            }
        }
        
        // Restore original solution vector
        solver->solutionVector = originalSolution;
        return true;
    } catch (...) {
        // Restore original solution vector
        solver->solutionVector = originalSolution;
        return false;
    }
}

// Directly calculate isolation between two specific ports (in dB)
bool IsolationCalculator::computePortIsolationInDB(const CouplingPort& sourcePort, const CouplingPort& observePort, double& isolationDB) {
    /**
     * Directly calculate isolation between two specific ports (in dB)
     * This is a convenience method that converts complex isolation to dB value
     */
    Complex isolation;
    if (!computePortIsolation(sourcePort, observePort, isolation)) {
        return false;
    }
    
    isolationDB = computeIsolationInDB(isolation);
    return true;
}

// Calculate port reflection coefficient
bool IsolationCalculator::computePortReflection(const CouplingPort& port, Complex& reflectionCoeff) {
    /**
     * Calculate port reflection coefficient
     * Reflection coefficient is defined as: Γ = (Zin - Z0) / (Zin + Z0)
     * where Zin is the port input impedance and Z0 is the port characteristic impedance
     */
    if (!solver->isInitialized) {
        return false;
    }
    
    try {
        // Compute port impedance
        std::vector<Complex> portImpedance;
        if (!computePortImpedance(port, portImpedance)) {
            return false;
        }
        
        // Calculate input impedance
        Complex zIn = 0.0;
        for (const auto& z : portImpedance) {
            zIn += z;
        }
        
        // Calculate reflection coefficient
        Complex z0 = port.impedance;
        if (std::abs(zIn + z0) > 1e-12) {
            reflectionCoeff = (zIn - z0) / (zIn + z0);
        } else {
            reflectionCoeff = 0.0; // Avoid division by zero error
        }
        
        return true;
    } catch (...) {
        return false;
    }
}

// Calculate port impedance distribution
bool IsolationCalculator::computePortImpedance(const CouplingPort& port, std::vector<Complex>& impedanceVector) {
    /**
     * Calculate port impedance distribution, returning impedance values for each basis function related to the port
     * These impedance values are used to generate normalized port excitation and calculate port current
     */
    if (!solver->isInitialized) {
        return false;
    }
    
    try {
        // Initialize impedance vector to appropriate size
        impedanceVector.resize(solver->numUnknowns, 0.0);
        
        // Calculate basis functions near the port
        const double portRadius = 0.01; // Port induction radius (adjustable based on specific conditions)
        
        for (size_t i = 0; i < solver->basisFunctions.size(); ++i) {
            const auto& basisFunc = solver->basisFunctions[i];
            
            // Calculate distance from basis function center to port position
            double distance = computeDistance(basisFunc.center, port.position);
            
            // If basis function is near the port
            if (distance <= portRadius) {
                // Calculate angle between basis function and port direction (using E-field orientation)
                double angle = computeAngle(basisFunc.normal, port.orientation[0]);
                
                // Calculate impedance value weighted by distance and angle
                double weight = std::exp(-distance / portRadius) * std::cos(angle);
                
                // Apply port impedance
                impedanceVector[i] = weight * port.impedance;
                
                // Consider the effect of material properties
                if (i < solver->materialProperties.size()) {
                    const auto& material = solver->materialProperties[i];
                    impedanceVector[i] *= material.conductivity;
                }
            }
        }
        
        return true;
    } catch (...) {
        return false;
    }
}

// Calculate distance between two points
double IsolationCalculator::computeDistance(const Vector_3D& point1, const Vector_3D& point2) {
    double dx = point2.x - point1.x;
    double dy = point2.y - point1.y;
    double dz = point2.z - point1.z;
    return std::sqrt(dx*dx + dy*dy + dz*dz);
}

// Calculate angle between two vectors
double IsolationCalculator::computeAngle(const Vector_3D& vec1, const Vector_3D& vec2) {
    // Calculate dot product of vectors
    double dotProduct = vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
    
    // Calculate magnitude of vectors
    double magnitude1 = std::sqrt(vec1.x*vec1.x + vec1.y*vec1.y + vec1.z*vec1.z);
    double magnitude2 = std::sqrt(vec2.x*vec2.x + vec2.y*vec2.y + vec2.z*vec2.z);
    
    // Avoid division by zero error
    if (magnitude1 < 1e-12 || magnitude2 < 1e-12) {
        return 0.0;
    }
    
    // Calculate cosine of the angle
    double cosAngle = dotProduct / (magnitude1 * magnitude2);
    
    // Ensure cosine value is within valid range [-1, 1]
    cosAngle = std::max(-1.0, std::min(1.0, cosAngle));
    
    // Return angle (in radians)
    return std::acos(cosAngle);
}


__declspec(dllexport) void* CreateIsolationCalculatorManager()
{
    return new IsolationCalculator(NULL);
}
} // namespace FieldCircuitCoupling